/*
 * SERVO.c
 *
 * Created: 07-07-2013 17:03:40
 *  Author: Karmu
 */ 

#include <math.h>
#include "servo.h"

#define SWEEP_DELAY 1300

uint8_t servo_max,servo_min,servo_mid;
uint8_t position;

void servo_pwm_timer2_enable(void)
{
	/*
	WGM21,WGM20 - Fast PWM Mode TOP=0xFF (Waveform Generation Mode 3), Update of OCR2 at BOTTOM and TOV2 Flag set on MAX
	COM21 - When Fast PWM mode Clear OC2A (Output Compare pin, PD7 on ATMEGA644p) on compare match, set OC2A at BOTTOM, (non-inverting mode)
	CS22,CS21,CS20 - Clock select to 1024 pre-scaler
	OCR2A Value decides the Duty Cycle
	*/
	TCNT2 = 0x00; // 8-bit counter
	TCCR2A |= _BV(WGM21) | _BV(WGM20) | _BV(COM2A1);
	TCCR2B |= _BV(CS22) | _BV(CS21) | _BV(CS20);
}

void servo_pwm_timer2_disable(void)
{
	TCCR2A = 0x00; // Stop
}

void servo_init(uint8_t s_max, uint8_t s_min)
{
	SERVO_DDR |= _BV(SERVO_CTRL);
	servo_max=s_max;
	servo_min=s_min;
	servo_pwm_timer2_disable();
	servo_pwm_timer2_enable();
	servo_set_position(90);
}

void servo_sweep(void)
{
	OCR2A=servo_min;
	_delay_ms(SWEEP_DELAY);
	OCR2A=servo_max;
	_delay_ms(SWEEP_DELAY);
}

void servo_set_position(uint8_t degree)
{
	degree=round(degree/10)*10;
	position=degree;
	OCR2A=servo_min+(degree/(180/(servo_max-servo_min)));
}

uint8_t servo_get_position(void)
{
	return position;
}